#include "freertos/FreeRTOS.h"
#include "freertos/queue.h"
#include "freertos/task.h"
#include "sdkconfig.h"
#include "stdio.h"
#include "string.h"
#include "stdlib.h"
#include "my_i2c.h"
#include "mpu6050.h"
#include "inv_mpu.h"
#include "led.h"

uint8_t buf[10] = {0};

void app_main(void)
{
    uint8_t t = 0;
    float pitch, roll, yaw;     //欧拉角
    int16_t aacx, aacy, aacz;   //加速度传感器原始数据
    int16_t gyrox,gyroy,gyroz;	//陀螺仪原始数据
    int16_t temp;

    led_init();
    MY_MPU6050_Init();
    while(mpu_dmp_init())
    {
        vTaskDelay(200 / portTICK_PERIOD_MS);
    }
    vTaskDelay(10 / portTICK_PERIOD_MS);

    // data[0] = 0x00;
    // MPU_Write_Len(0x1E, 0x02, 1, uint8_t *data)
    MPU_Write_Byte(0x1E, 0x02, 0x00);

    while (1)
    {
        if(mpu_dmp_get_data(&pitch, &roll, &yaw) == 0)
        {
            temp=MPU_Get_Temperature();	//得到温度值
            MPU_Get_Accelerometer(&aacx, &aacy, &aacz);	//得到加速度传感器数据
			MPU_Get_Gyroscope(&gyrox, &gyroy, &gyroz);	//得到陀螺仪数据

            MPU_Read_Len(0x12, 0x34, 2, buf);


            if((t % 10) == 0)
            {
                //温度temperature
                printf("the temperature is %d.%d\n", temp/100,temp%10);  

                //俯仰角pitch
                temp = pitch * 10;
                printf("the pitch is %d.%d\n", temp/10,temp%10);

                //横滚角roll
                temp = roll * 10;
                printf("the roll is %d.%d\n", temp/10,temp%10);

                //航向角yaw
                temp = yaw * 10;
                printf("the yaw is %d.%d\n", temp/10,temp%10);

                t = 0;
                printf("\n");
                vTaskDelay(100 / portTICK_PERIOD_MS); 
           }
        }
        t++;    
    } 
}
